/*!
  ******************************************************************************
  * @file           : main.c
  * @author         :He Kunling
  * @createDate     : 2021/07/31
  * @lastEditors    :
  * @lastEditTime   : 2021/07/31
  * @brief          : 
  * 
  *     This file provides functions bellow to be called:
  * 
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2021 Zero_One NUAA.
  * All rights reserved.
  *
  ******************************************************************************
  */

 #include "main.hpp"

remote_control_t remote_cmd;
 usr_flag_t usr_flag; 
 groundbase_cmd_t groundbase_cmd;
 flight_control_to_NX_data_t flight_control_to_NX_data;
 circleBuf_t groundbase_ringbuf;
 circleBuf_t flightcontrol_ringbuf;
 circleBuf_t SBUS_ringbuf;
 drone_data_to_groundbase_t drone_data_to_groundbase;
 user_time_t curr_time;


 uint16_t _channels[16] = {0};

waypoint_groundbase_t groundbase_waypoint;//用来储存地面站的航点信息
waypoint_flight_control_t flight_control_waypoint;//用来储存飞控回传的航点信息
 target_vel_t target_velocity;   ///用来存储期望的速度




bool ground_cmd_flag = false;

 bool NX_get_start_up_waypoint_flag = false;   ////接收到地面站开始上传航点标志位
 bool NX_get_up_waypoint_mess_flag = false;    ///得到飞控单个上传航点信息标志位
 bool NX_get_up_waypoint_single_flag = false;   ///接收到地面站单个上传航点指令标志位
 bool NX_get_up_waypoint_final_flag = false;    ///最后一个航点标志位
 uint8_t NX_get_up_down_waypoint_cnt = 0;

 bool if_up_and_down_waypoint_falg = false;




uint16_t TF_mini_data = 0;
uint16_t TF_strength = 0;
int file;

int UDP_fd;

 int flight_control_lose_massage_cnt;
 bool flight_control_lose_massage_flag;
 void usr_para_init(void)
 {
      usr_flag.flight_ID = 0x01;

      //for test !!!!
      //A1
      drone_data_to_groundbase.selfcheck = 0;
      drone_data_to_groundbase.flight_mode = 0;
      drone_data_to_groundbase.flight_status =0;
      drone_data_to_groundbase.fix_status = 0;

      drone_data_to_groundbase.battery = 0;
      drone_data_to_groundbase.equip_status = 0;
  
      //A2
      drone_data_to_groundbase.roll = 0;
      drone_data_to_groundbase.pitch = 0;
      drone_data_to_groundbase.yaw =0;

      drone_data_to_groundbase.vel.x = 0;
      drone_data_to_groundbase.vel.y = 0;
      drone_data_to_groundbase.vel.z = 0; 

      drone_data_to_groundbase.curr_pos.lat = 0;
      drone_data_to_groundbase.curr_pos.lng = 0;

      drone_data_to_groundbase.curr_pos.alt = 0;

/*******************************地面站发***************************************/
      groundbase_cmd.control_cmd = 1;       //控制命令
      groundbase_cmd.query_cmd = 0;                    //查询命令
      groundbase_cmd.roll_control = 0;                   //滚转 
      groundbase_cmd.pitch_control = 0;               //俯仰     
      groundbase_cmd.pan_tilt_pitch = 0;             //云台俯仰
      groundbase_cmd.pan_tilt_point_to = 0;     //云台指向
      groundbase_cmd.point_to_control = 0;      //飞机指向
      groundbase_cmd.acc_control = 0;                //飞机油门
      groundbase_cmd.current_up_index = 0; 
/******************************************************************************/
target_velocity.target_vel_x = 0;
target_velocity.target_vel_y = 0;



      usr_flag.control_flag = false;
      usr_flag.flight_point_flag = false;
      usr_flag.query_1 = false;
      usr_flag.query_2 = false;
      usr_flag.query_3 = false;
      usr_flag.query_4 = false;
      usr_flag.query_7 = false;
      usr_flag.query_8 = false;
      usr_flag.final_waypoint_flag = false;
      // usr_flag.SBUS_flag = false;
     usr_flag.NX_wait_groundbase_controlcmd_ready_flag = 3;
     usr_flag.remote_lose_rc_link_flag = false;
     usr_flag.gcs_lose_heart_beat_flag = false;
     usr_flag.NX_get_groundbase_heartbeat_cnt = 0;

     usr_flag.remote_control_flag = false;

      usr_flag.onboard_land_flag = false;
      usr_flag.onboard_takeoff_flag = false;


   flight_control_lose_massage_cnt = 0;
   flight_control_lose_massage_flag = false;

 }

uint16_t CalculateCrc16(volatile uint8_t *ptr,uint8_t len)    
{   
	uint16_t crc;   
	uint8_t da;   
   	uint16_t crc_ta[16]={    
	0x0000,0x1021,0x2042,0x3063,0x4084,0x50a5,0x60c6,0x70e7,   
	0x8108,0x9129,0xa14a,0xb16b,0xc18c,0xd1ad,0xe1ce,0xf1ef,   
	};   
	crc=0;   
	while(len--!=0)   
	{   
		da=crc>>12;
		crc<<=4;
		crc^=crc_ta[da^(*ptr>>4)];
		da=crc>>12;
		crc<<=4;
		crc^=crc_ta[da^(*ptr&0x0F)];
		ptr++;
	}
	return(crc);
}

uint8_t get_BCC(uint8_t* data,uint16_t length)
{
      uint8_t bcc = 0x00;
      for(uint16_t i=1;i<length;)
      {
            bcc ^= data[i++];
      }

      return bcc;

}
